Navigation augmentation system and method

ABSTRACT

A navigation augmentation system includes a vehicle including an imaging device operably connected to a navigation data fusion module for receiving and analyzing visual point of interest data, gyroscope data, and accelerometer data, wherein the navigation data fusion module is operably connected to a sensor compensation module and an autopilot module for controlling the navigation of the vehicle.

BACKGROUND Technological Field

The present disclosure relates to a navigation augmentation system, andmore particularly to a navigation augmentation system without using GPS.

Description of Related Art

A variety of devices are known in the navigation systems for vehicles.Precision guided munitions require accurate estimates of position,velocity, and attitude in order to hit a designated target. Currentweapon systems rely on GPS and high cost IMUs. The new paradigm forprecision guided munitions is GPS-denied, low cost and volume IMUs, andmaximum airframe performance. This patent eliminates major IMU errorsources and enables use of IMUs with lower cost and volume than wouldotherwise be required.

The conventional methods and systems have generally been consideredsatisfactory for their intended purpose. However, there is still a needin the art for low-cost navigation systems operating in a GPS-deniedenvironment but maintaining accurate position, velocity, and attitudeestimates, which in turn improve seeker target acquisition capabilities.There also remains a need in the art for such systems and componentsthat are economically viable. The present disclosure may provide asolution for at least one of these remaining challenges.

SUMMARY OF THE INVENTION

A navigation augmentation system includes a vehicle, such as anaircraft, missile or projectile, including an imaging device operablyconnected to a navigation data fusion module configured for receivingand analyzing visual point of interest data, gyroscope data, andaccelerometer data, wherein the navigation data fusion module isoperably connected to a sensor compensation module and an autopilotmodule for controlling navigation of the vehicle. The accelerometer andthe gyroscope can also be directly and operably connected to the sensorcompensation module. The navigation data fusion module can include aKalman filter.

The imaging device can include a series of cameras which can beinterconnected or independent of each other, each communicating with thenavigation data fusion module. Each imaging device can include a horizonsensor.

A method of augmenting navigation of a vehicle is also disclosed. Themethod includes updating a vehicle control command due to receiving datafrom an autopilot module and data from sensors compensation module forcontrolling a direction of travel of a vehicle wherein the sensorcompensation module receives bias and scale factor error estimates for agyroscope and bias and scale factor error estimates for an accelerometerfrom a navigation data fusion system wherein the navigation data fusionsystem receives and aggregates input from an integrator for a gyroscope,an integrator for an accelerometer, and an imaging device, and thevehicle changes direction based on the navigation updates. Roll andpitch of the vehicle can be determined by the imaging device when usedas a horizon sensor, and can be used as inputs to the navigation datafusion system. The roll and pitch of the vehicle can be coupled with thevisual points of interest data as an input to the data fusion system.

The imaging device can detect at least one point of interest in a firstimage and track the point of interest in a second image using analgorithm. The algorithm can be a SIFT (Scale Invariant FeatureTransform), FAST (Features from Accelerated Segment Test) algorithm, orsimilar algorithm.

The method described above is intended to be used in areas where GPSdata is inaccessible or intermittently accessible; as such the bias andscale factor error estimates are calculated without using GPS data, andspecifically the vehicle can operate in a GPS denial area.

These and other features of the systems and methods of the subjectdisclosure will become more readily apparent to those skilled in the artfrom the following detailed description of the preferred embodimentstaken in conjunction with the drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

So that those skilled in the art to which the subject inventionappertains will readily understand how to make and use the devices andmethods of the subject invention without undue experimentation,preferred embodiments thereof will be described in detail herein belowwith reference to certain figures, wherein:

FIG. 1 is a schematic view of a navigation augmentation system; and

FIG. 2 is a 2-D Camera Rotation/Translation Diagram for the navigationaugmentation system of FIG. 1.

DETAILED DESCRIPTION

Reference will now be made to the drawings wherein like referencenumerals identify similar structural features or aspects of the subjectinvention. For purposes of explanation and illustration, and notlimitation, a partial view of an exemplary embodiment of a navigationaugmentation system in accordance with the invention is shown in FIG. 1and is designated generally by reference character 100. The methods andsystems of the invention can be used to bound the change in position andattitude by estimating the bias and scale factor errors of theaccelerometers and gyroscopes of a vehicle in flight.

During flight, the imaging seeker is used to capture images at fixedintervals. Each recorded image is scanned for points of interest (POI)which can be tracked or matched in successive images. Given many suchpoints, a least-squares estimate of the camera's change in roll, pitch,yaw, and translation direction can be calculated for each pair ofimages. The translation direction vector must be multiplied by a scalingfactor, α, which can be determined using an additional informationsource, i.e. an altimeter, velocity, or position measurement within theimage. These can be used as aiding information to enhance navigationestimates.

A navigation augmentation system 100 includes a vehicle 101,specifically an airborne vehicle such aircraft or a munition, includingan imaging device 102 operably connected to a navigation data fusionmodule 104 configured for receiving and analyzing delta rotations anddelta translations (from block 3), gyroscope data, and accelerometerdata. The navigation data fusion module 104 is operably connected to asensor compensation module 106 and an autopilot module 108 forcontrolling the vehicle 101. An accelerometer 110 and a gyroscope 112are also directly and operably connected to the sensor compensationmodule 106. The navigation data fusion module 104 could be implementedas a Kalman filter.

The imaging device 102 can include a series of cameras which can beinterconnected or independent of each other, each with independenttranslation/rotation estimation modules ((1)-(3)), each providing deltatranslation/rotation angles to the navigation data fusion module 104.Each imaging device can be used as a horizon sensor.

A method of augmenting navigation of a vehicle is also disclosed. Anyfeature detection method that provides a list of unique points can beused, so long as the same point can be detected or tracked in the secondimage, i.e. the FAST or the SIFT algorithm. The output of an imagingcollection can be a matrix of points, p in homogeneous coordinates:

$p = \begin{bmatrix}p_{1x} & p_{2x} & \ldots & p_{nx} \\p_{1y} & p_{2y} & \ldots & p_{ny} \\1 & 1 & \ldots & 1\end{bmatrix}$Where p_(nx) is the pixel column of the n^(th) point and p_(ny) is thepixel row of the n^(th) point. Each of these homogeneous points can betreated as a 3-dimensional pointing vector from the optical center ofthe camera towards the point in 3D space, where the camera is at theorigin and its center pixel points along the Z axis, where the X axisaligns to the image rows and the Y axis aligns to the image columns.

For each of the points identified in the first image, a matching pointmust be located in the second image. In the case where there isrelatively little motion between successive camera frames, as in a highframe-rate video stream, the points can be located using a trackingalgorithm. One method for tracking is to consider a small region ofpixels around the POI of the first image and search the second image forthe nearby region with the highest correlation:

$\left\lbrack {p_{x}^{\prime}\mspace{20mu} p_{y}^{\prime}} \right\rbrack = {\underset{x,y}{argmax}{\sum\limits_{R}{I\left( {p_{x},{p_{y)}{I^{\prime}\left( {x,y} \right)}}} \right.}}}$Where:p_(x)′ and p_(y)′ are the corresponding column and row of point p in thesecond imageR is a small region of pixels around pI is the first imageI′ is the second imageOther methods, such as the method above but with a Kalman filter, or theKanade-Lucas-Tomasi (KLT) tracker can be used. As an alternative tofeature tracking, feature matching can also be performed.The output of this step is a second matrix of points, p′ in homogeneouscoordinates:

$p^{\prime} = \begin{bmatrix}p_{1x}^{\prime} & p_{2x}^{\prime} & \ldots & p_{nx}^{\prime} \\p_{1y}^{\prime} & p_{2y}^{\prime} & \ldots & p_{ny}^{\prime} \\1 & 1 & \ldots & 1\end{bmatrix}$Where a correspondence between the points is maintained via indices. Ifa matching point cannot be located in the second image, it is removedfrom the list.

As shown in FIG. 2, for every point in 3-dimensional space that isobserved from two different camera locations, a plane can be definedusing the point P, and the optical centers (lenses) of each camera—O andO′.

The line OO′ is the direction of translation between frames k and k+1,and the lines OP and O′P are the lines connecting each camera's opticalcenter to the point in 3-dimensional space. Since these points define aplane, the following relation holds:{right arrow over (OP)}·[{right arrow over (OO′)}×{right arrow over(O′P)}]=0This can be re-written using the coordinates of the first camera as:p·[t×(Rp′)]=0Where t is the translation and R is the rotation matrix from the secondcamera orientation to the first camera orientation. The essential matrixis defined as:ε=[t×]RWhere [t×] is the skew symmetric form of t, which is the implementationof the cross-product with a matrix multiplication. The essential matrixis defined using normalized camera coordinates, accounting for thecamera's intrinsic parameters (focal length, etc.) The transformationbetween the essential matrix and the fundamental matrix is linear:

=K ^(−T) εK ⁻¹Where K is the camera calibration matrix. This will be defined andhandled in the following step. For the moment, for each point (definedin pixel coordinates) that exists in both images, the following relationholds:p ^(T)

p′=0Using the set of point correspondences acquired in the previous steps, aleast-squares estimate of the fundamental matrix can be computed, whichcontains the desired rotation and translation information.

The equation can be rewritten to allow for a solution using standardlinear least-squares optimization techniques:

${{\left\lbrack {x\mspace{20mu} y\mspace{23mu} 1} \right\rbrack\begin{bmatrix}\mathcal{F}_{11} & \mathcal{F}_{12} & \mathcal{F}_{13} \\\mathcal{F}_{21} & \mathcal{F}_{22} & \mathcal{F}_{23} \\\mathcal{F}_{31} & \mathcal{F}_{32} & \mathcal{F}_{33}\end{bmatrix}}\begin{bmatrix}x^{\prime} \\y^{\prime} \\1\end{bmatrix}} = 0$This is a system of nine homogeneous linear equations:UF=0Where

$U = \begin{bmatrix}{x\; 1x\; 1^{\prime}} & {x\; 1y\; 1^{\prime}} & {x1} & {y\; 1x\; 1^{\prime}} & {y\; 1y\; 1^{\prime}} & {y1} & {x\; 1^{\prime}} & {y\; 1^{\prime}} & 1 \\{x\; 2x\; 2^{\prime\;}} & {x\; 2y\; 2^{\prime}} & {x2} & {y\; 2x\; 2^{\prime}} & {y\; 2y\; 2^{\prime}} & {y2} & {x\; 2^{\prime}} & {y\; 2^{\prime}} & 1 \\\vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\{xnxn}^{\prime} & {xnyn}^{\prime} & {xn} & {ynxn}^{\prime} & {ynyn}^{\prime} & {yn} & {xn}^{\prime} & {yn}^{\prime} & 1\end{bmatrix}$AndF=[

₁₁

₁₂

₁₃

₂₁

₂₂

₂₃

₃₁

₃₂

₃₃]^(T)The fundamental matrix can now be estimated using standard linearleast-squares techniques. The fundamental matrix estimate does notaccount for the camera's focal length, focal center, and pixel size,however, it is linearly related to the desired essential matrix:ε=K ^(T)

K′Where K is the camera calibration matrix, and predetermined, orsometimes called the intrinsic matrix, is defined as:

$K = \begin{bmatrix}f & 0 & c_{x} \\0 & f & c_{y} \\0 & 0 & 1\end{bmatrix}$

Where f is the focal length expressed in number of pixels and c_(x) andc_(y) are the pixel column and row aligned with the lens's optical axis.

To extract the rotation and translation vectors from the essentialmatrix, the singular value decomposition is utilized:ε=UWV ^(T)Where U and V are rotation matrices and W is a diagonal matrixcontaining the singular values. Due to the defined constraints of theessential matrix, it must have rank 2, and therefore only contain 2singular values. This means that the third column of U defines the nullspace of ε. Since ε is defined as the product of one rotation matrix andone skew-symmetric matrix representing the translation vector, thevector t′ is the null space of ε and therefore the 3^(rd) column of U.It is only defined by its direction, and at the moment it could bepositive or negative.

There are also two possibilities for the rotation matrix R depending onthe ordering of the first two columns of U. Using the four totalcombinations of R and t′, only one of these will result in the pointsbeing in front of the camera in both positions, and this provides theestimate of R and t′. The translation vector t′, is a unit vector,therefore the scaling factor, α, must be multiplied by t′ in order tohave a properly scaled delta relative position.t=αt′This information can then be used to extract the delta attitude anddelta relative position between frames, which can then be applied as anaiding source to a navigation algorithm.

The method includes updating a vehicle control command from an autopilotmodule 108 and data from sensors compensation module 106 for controllinga direction of travel of a vehicle wherein the sensor compensationmodule 106 receives bias and scale factor error estimates for agyroscope 112 and bias and scale factor error estimates for anaccelerometer 110 from a navigation data fusion system 104 wherein thenavigation data fusion system 104 receives an aggregate input from anintegrator for a gyroscope 113, an integrator for an accelerometer 111,and an imaging device 102, and the vehicle changes direction based onthe navigation updates. Change in position, velocity and attitude of thevehicle are determined by the accelerometer and the gyroscopeintegrators and used as inputs to the navigation data fusion system. Thevisual points of interest data are processed to estimate deltatranslation and rotation of the imager, which is used as an input to thedata fusion system. The data fusion system uses the delta attitude anddelta relative positions to compare to the attitude and position changesestimated by the gyroscope and accelerometers. The difference betweenthe two is specified as bias and scale factor error of the gyroscope andaccelerometers, as the imaging device has bias-free errorcharacteristics. The sensor fusion module uses these error terms tocompensate the gyroscope and accelerometer outputs. This gives thecompensated gyroscope output (ω_(comp,1) ^(b)) and compensatedaccelerometer output (α_(comp,1) ^(b)) as:

${\omega_{{comp},1}^{b} = \frac{\omega_{{out},1}^{b} - b_{1}^{g}}{k_{1}^{g}}}{a_{{comp},1}^{b} = \frac{a_{{out},1}^{b} - b_{1}^{a}}{k_{1}^{a}}}$

The imaging device 102 can detect at least one point of interest in afirst image and track the point of interest in a second image. Thedetection algorithm can be the SIFT or the FAST algorithm. The systemuses an imaging seeker to track multiple distinguishable terrainfeatures (i.e. rivers, lakes, roads, shoreline, rock formations,vegetation, etc) which are assumed to be on the surface, and inertiallyfixed. An imaging sensor provides angle-only measurements (with respectto the body) to each feature. Tracking these features allowsdelta-position, delta-attitude, and velocity to be bounded whilenavigating via dead-reckoning by using features as an aid to navigation.

This system and method enhance integrated guidance units to operate byimproving attitude and position estimates and seeker acquisitioncapabilities. The systems allows for using small, low cost sensors,which can be calibrated post launch. This system and method disclosedabove remove gyro and accelerometer scale factor and bias, which aremajor error sources of low cost units, and improve navigation, airframestability, and guidance accuracy.

The method described above is intended to be used in areas where GPSdata is inaccessible or intermittently accessible, as such the bias andscale factor error estimates are calculated without using GPS data.

The methods and systems of the present disclosure, as described aboveand shown in the drawings, provide for flight systems with superiorproperties including increased reliability and stability, and reducedsize, weight, complexity, and/or cost. While the apparatus and methodsof the subject disclosure have been showing and described with referenceto embodiments, those skilled in the art will readily appreciate thatchanges and/or modifications may be made thereto without departing fromthe spirit and score of the subject disclosure.

The invention claimed is:
 1. A navigation augmentation system comprising: a vehicle including an imaging device comprising a plurality of cameras operably connected to a navigation data fusion module configured for receiving and analyzing visual point of interest data, gyroscope data, and accelerometer data; wherein the navigation data fusion module is operably connected to a sensor compensation module and an autopilot module for controlling the navigation of the vehicle, and the imaging device collects imaging data including a matrix of points, p in homogeneous coordinates: $p = \begin{bmatrix} p_{1x} & p_{2x} & \ldots & p_{nx} \\ p_{1y} & p_{2y} & \ldots & p_{ny} \\ 1 & 1 & \ldots & 1 \end{bmatrix}$ where p_(nx) is a pixel column of an n^(th) point and p_(ny) is a pixel row of the n^(th) point and wherein a region of pixels around a point of interest of a first image and a second image includes: $\left\lbrack {p_{x}^{\prime}\mspace{20mu} p_{y}^{\prime}} \right\rbrack = {\underset{x,y}{argmax}{\sum\limits_{R}{I\left( {p_{x},{p_{y)}{I^{\prime}\left( {x,y} \right)}}} \right.}}}$ where: p_(x)′ and p_(y)′ are corresponding column and row of point p in the second image, R is the region of pixels around p, I is the first image, and I′ is the second image.
 2. The navigation augmentation system of claim 1, wherein the accelerometer is operably connected to the sensor compensation module.
 3. The navigation system of claim 1, wherein the gyroscope is operably connected to the sensor compensation module.
 4. The navigation system of claim 1, wherein navigation data fusion module includes a Kalman filter.
 5. The navigation system of claim 1, wherein the imaging device includes a series of independent cameras.
 6. The navigation system of claim 1, wherein the imaging device includes a horizon sensor. 